#include <ros/ros.h>
#include <navigation/navigation.h>
#include <algorithm>
#include <termios.h>
#include <unistd.h>
#include <stdio.h>
#include <fcntl.h>


using namespace std;

Navigation::Navigation(float startX, float startY, float startYaw,
                       unsigned char leftStop, unsigned char rightStop,
                       unsigned char leftLowRange, unsigned char rightLowRange,
                       unsigned char leftHighRange, unsigned char rightHighRange)
: position(startX, startY, 0.0),
  yaw(startYaw),
  lineOffset(0.0),
  referenceAngle(startYaw),
  targetAngle(startYaw),
  angularLee(0.523598776),
  leftZero(leftStop),
  rightZero(rightStop),
  maxLinearSpeed(0.5),
  maxAngularSpeed(0.32051282051282*TWO_PI),
  leftMaxValue(leftHighRange),
  rightMaxValue(rightHighRange),
  leftMinValue(leftLowRange),
  rightMinValue(rightLowRange),
	deadZoneDistance(0.01)
{
	lastUpdate = ros::Time::now();
	waypoint.x = startX;
	waypoint.y = startY;
	leftMaxValue = std::min<unsigned char>(leftMaxValue, 255-leftZero);
	rightMaxValue = std::min<unsigned char>(rightMaxValue, 255-rightZero);
	leftMinValue = std::min<unsigned char>(leftZero, leftMinValue);
	rightMinValue = std::min<unsigned char>(rightZero, rightMinValue);
}

Navigation::~Navigation()
{

}

// Uses linear-only interpolation. Make sure you call frequently
// Something is wrong with my calculus skills tonight.
void Navigation::updatePosition()
{
	//Update pose
	ros::Duration deltaTime = ros::Time::now() - lastUpdate;
	lastUpdate = ros::Time::now();
	double seconds = deltaTime.toSec();
	float speed = getLinearSpeed();
	position.setX( position.x() + seconds*speed*cos(yaw) );
	position.setY( position.y() + seconds*speed*sin(yaw) );
	float tempYaw = yaw + getAngularSpeed()*seconds;
	yaw = std::fmod(TWO_PI + tempYaw, TWO_PI);
	
	//Update target angle
	float deltaX = waypoint.x - position.x();
	float deltaY = waypoint.y - position.y();
	targetAngle = std::fmod(TWO_PI + atan2(deltaY, deltaX), TWO_PI);

	//Update reference angle if we overshoot
	float safety = 0.1;
	if(((referenceAngle > 0.75*TWO_PI || referenceAngle < 0.25*TWO_PI) && position.x() > waypoint.x+safety)
	|| ((referenceAngle > 0 && referenceAngle < 0.5*TWO_PI) && position.y() > waypoint.y+safety)
	|| ((referenceAngle > 0.25*TWO_PI && referenceAngle < 0.75*TWO_PI) && position.x() < waypoint.x-safety)
	|| ((referenceAngle > 0.5*TWO_PI && referenceAngle < TWO_PI) && position.y() < waypoint.y-safety))
	{
		referenceAngle = std::fmod(referenceAngle + 0.5*TWO_PI, TWO_PI);
	}
}

float Navigation::getDistance()
{
	float deltaX = waypoint.x - position.x();
	float deltaY = waypoint.y - position.y();

	return sqrt(deltaX*deltaX + deltaY*deltaY);
}

float Navigation::getLinearSpeed()
{
	if( getDistance() <= deadZoneDistance)
	{
		//ROS_DEBUG("Close to waypoint. Stopping.");
		return 0;
	}

	if(!angleInLeeway(yaw, targetAngle))
	{
		//ROS_DEBUG("Out of alignment. Stopping.");
		return 0.0;
	}


	//TODO: A P controller here is really just a cop-out...
	/*float output = 0;
	if( getDistance() > 0.25)
	{
		output = maxLinearSpeed;
	}
	else
	{
		output =  maxLinearSpeed * std::min<float>(getDistance(), 1.0);
	}*/

	return maxLinearSpeed;
}

float Navigation::getAngularSpeed()
{
	if( getDistance() <= deadZoneDistance)
	{
		//ROS_DEBUG("Close to waypoint. Not Turning.");
		return 0;
	}

	float diff = targetAngle - yaw;
	float rawAngleDiff = fabs(diff);
	float angleDiff = std::min<float>( rawAngleDiff, TWO_PI-rawAngleDiff );

	if(angleDiff <= TWO_PI/180)
	{
		//ROS_DEBUG("Close enough.");
		return 0;
	}

	//TODO: Really, I should know better then this
	float output;

	float referenceFrame = (TWO_PI - targetAngle) + yaw;
	referenceFrame = 
	    referenceFrame >= TWO_PI ? referenceFrame - TWO_PI : referenceFrame;

	//ROS_DEBUG("Relative Yaw: %f", referenceFrame);

	if(referenceFrame < (TWO_PI/2.0))
	{
		output = -TWO_PI/4 * (angleDiff/angularLee + lineOffset/320);
		output = std::max<float>(output, -maxAngularSpeed);
		//ROS_DEBUG("CLOCKWISE: %f", output);
	}
	else
	{
		output = TWO_PI/4 * (angleDiff/angularLee + lineOffset/320);
		output = std::min<float>(output, maxAngularSpeed);
		//ROS_DEBUG("COUNTER-CLOCKWISE: %f", output);
	}

	return output;
}

unsigned char Navigation::getLeftWheelSpeed()
{
	float angularSpeed = getAngularSpeed();
	float linearSpeed = getLinearSpeed();
	unsigned char range = leftMaxValue+leftMinValue;

	int baseSpeed = -range*(linearSpeed/maxLinearSpeed) + leftZero;
	if(angularSpeed >= 0)
	{
		baseSpeed -= -range*(angularSpeed/maxAngularSpeed);
	}
	else
	{
		baseSpeed += -range*(fabs(angularSpeed)/maxAngularSpeed);
	}

	//ROS_DEBUG("LEFT: %d", baseSpeed);

	if(baseSpeed == leftZero)
	{
		return 0;
	}

	int output = baseSpeed;
	
	if(baseSpeed < leftZero - leftMinValue)
	{
		output =  leftZero-leftMinValue;
	}

	if(baseSpeed > leftZero + leftMaxValue)
	{
		output =  leftZero + leftMaxValue;
	}

	//ROS_DEBUG("LEFT 2: %d", output);

	return output;
}

unsigned char Navigation::getRightWheelSpeed()
{
	float angularSpeed = getAngularSpeed();
	float linearSpeed = getLinearSpeed();
	unsigned char range = rightMaxValue+rightMinValue;
	
	int baseSpeed = range*(linearSpeed/maxLinearSpeed) + rightZero;
	if(angularSpeed <= 0)
	{
		baseSpeed -= range*(fabs(angularSpeed)/maxAngularSpeed);
	}
	else
	{
		baseSpeed += range*(angularSpeed/maxAngularSpeed);
	}

	//ROS_DEBUG("RIGHT: %d", baseSpeed);

	if(baseSpeed == rightZero)
	{
		return 0;
	}

	int output = baseSpeed;

	if(baseSpeed < rightZero - rightMinValue)
	{
		output = rightZero - rightMinValue;
	}

	if(baseSpeed > rightZero + rightMaxValue)
	{
		output = rightZero + rightMaxValue;
	}

	//ROS_DEBUG("RIGHT 2: %d", output);

	return output;
}

void Navigation::lineCallback(const geometry_msgs::PointConstPtr& lineParams)
{
	//ROS_INFO("Entering lineCallback");	

	updatePosition(); // Update position

	double offset = lineParams->x;
	double slope = lineParams->y;;
	
	//TODO: Real math here, please.
	lineOffset = 0; //Forgot to map spherical lens... offset is both angular and linear, slope changes more with linear offset
	
	// Check if we are close enough the reference line to servo on it
	if( angleInLeeway(yaw, referenceAngle) )
	{ // Derive position relative to line if we believe it is in view
		const float cameraFOV = TWO_PI/12; //GET RID OF THIS HACK
		float angularDeviation = offset/320/cameraFOV; //Angle is more indicated by deviation than slope
		if(angularDeviation < 0)
		{
			angularDeviation = TWO_PI + angularDeviation; // Modulo math...
		}
	  	float tempYaw = std::fmod(referenceAngle + angularDeviation, TWO_PI);
		tempYaw = (TWO_PI - yaw) + tempYaw;
		tempYaw = tempYaw >= TWO_PI ? tempYaw - TWO_PI : tempYaw;

		if(tempYaw < TWO_PI/2)
		{
			yaw = tempYaw/2 + yaw;
		}
		else
		{
			tempYaw = (tempYaw + TWO_PI)/2 + yaw;
			tempYaw = tempYaw >= TWO_PI ? tempYaw - TWO_PI : tempYaw;
		}
		
		ROS_DEBUG("Line Yaw: %f", yaw);
	}
}

void Navigation::waypointCallback(const geometry_msgs::PointConstPtr& goal)
{
	//ROS_INFO("Entering waypointCallback");
	waypoint = *goal;
	float tempRef = atan2(goal->y - position.y(), goal->x - position.x());
	referenceAngle = std::fmod(TWO_PI + tempRef, TWO_PI);
}

bool Navigation::angleInLeeway(float angle, float reference)
{
	float diff = fabs(angle-reference);
	if( diff <= angularLee || TWO_PI - diff <= angularLee)
		return true;
	return false;
}

// in Main: while(1) position.integrate();
// steering.updatePosition(position.getPose());
// write(steering.getLeft()); write(steering.getRight());
// publish(position.getPose());

int main(int argc, char* argv[]){
	// Start ROS connection
	ros::init(argc, argv, "seeker_navigation");
	ros::NodeHandle node;
	ros::NodeHandle pvNode("~");

	char* leftdevice = 0;
	char* rightdevice = 0;
	if( argc > 1 )
	{
		leftdevice = argv[1];
		rightdevice = argv[2];
	}
	else
	{
		ROS_FATAL("Input requires two /dev/ttyACM* for communicating to motor "
		       "controller as first two arguments.");
	}

	ROS_DEBUG("Initializing navigation class");
	
	double startX, startY, startYaw;
	int leftZero, rightZero, leftLowRange, rightLowRange, leftHighRange, rightHighRange;
	pvNode.param("start_x", startX, 0.0);
	pvNode.param("start_y", startY, 0.0);
	pvNode.param("start_yaw", startYaw, 0.0);
	pvNode.param("left_zero", leftZero, 160);
	pvNode.param("right_zero", rightZero, 160);
	pvNode.param("left_low_range", leftLowRange, 160);
	pvNode.param("right_low_range", rightLowRange, 20);
	pvNode.param("left_high_range",  leftHighRange, 20);
	pvNode.param("right_high_range", rightHighRange, 20);

	Navigation navigation(startX, startY, startYaw, 
	                      (unsigned char)leftZero, (unsigned char)rightZero, 
	                      (unsigned char)leftLowRange, (unsigned char)rightLowRange,
	                      (unsigned char)leftHighRange, (unsigned char)rightHighRange);

 	// Register node and advertise the heartbeat
	ROS_DEBUG("Creating Pub/Sub Interfaces");
	ros::Publisher posePublisher = node.advertise<geometry_msgs::PoseStamped>("seeker_pose", 2);
	ros::Subscriber waypoints = node.subscribe("waypoints_in", 10, &Navigation::waypointCallback, &navigation);
	ros::Subscriber lines = node.subscribe("line_in", 10, &Navigation::lineCallback, &navigation);

	// Open connection to motor controller
	int leftfd = open(leftdevice, O_WRONLY | O_NOCTTY);
	int rightfd = open(rightdevice, O_WRONLY | O_NOCTTY);
	if((leftfd | rightfd) < 0)
	{
		ROS_ERROR("Failed to open connection to motor controller.");
		exit(-1);
	}
	
	ROS_DEBUG("Configuring serial");
	// Configure serial settings
	struct termios portSettings;
	cfmakeraw(&portSettings);
	cfsetspeed(&portSettings, B9600);
	portSettings.c_cflag = CS8 | CRTSCTS | CLOCAL; //8N1
	tcsetattr(leftfd, TCSAFLUSH, &portSettings);
	tcsetattr(rightfd, TCSAFLUSH, &portSettings);

	ROS_DEBUG("Initializing transform broadcaster");
	// Create broadcaster for heartbeat
  	tf::TransformBroadcaster br;

	// Set operating rate (must be at least 50 Hz for proper servo control)  
	ros::Rate rate(60);
	bool flipflop = true;
	unsigned char leftWheelSpeed = -1;
	unsigned char rightWheelSpeed = -1;
	while(node.ok())
	{
		ros::spinOnce();
		navigation.updatePosition();
		if(flipflop)
		{
			unsigned char wheelSpeed = navigation.getLeftWheelSpeed();
			if(wheelSpeed != leftWheelSpeed)
			{
				leftWheelSpeed = wheelSpeed;
				//ROS_DEBUG("Left Speed: %d", (int)leftWheelSpeed);
				size_t leftOutBytes = write(leftfd, &leftWheelSpeed, sizeof(unsigned char));
				if(leftOutBytes != sizeof(unsigned char))
				{
					ROS_ERROR("Grue ate the bytes.");
				}
			}
		}
		else
		{
			unsigned char wheelSpeed = navigation.getRightWheelSpeed();
			if(wheelSpeed != rightWheelSpeed)
			{
				rightWheelSpeed = wheelSpeed;
				//ROS_DEBUG(" Right Speed: %d", (int)rightWheelSpeed);
				size_t rightOutBytes = write(rightfd, &rightWheelSpeed, sizeof(unsigned char));
				if(rightOutBytes != sizeof(unsigned char))
				{
					ROS_ERROR("I meant to do that");
				}
			}
		}
		flipflop = !flipflop;
	
		tf::Pose pose;
		pose.setOrigin(navigation.getPosition());
		pose.setRotation( tf::createQuaternionFromYaw(navigation.getYaw()) );

		br.sendTransform( tf::StampedTransform(pose, ros::Time::now(), "world",
		                  "seeker") );

		tf::Stamped<tf::Pose> stampedPose(pose, ros::Time::now(), "world");
		geometry_msgs::PoseStamped outPose;
		tf::poseStampedTFToMsg(stampedPose, outPose);
		posePublisher.publish(outPose);

   
 		rate.sleep();
	}

  return 0;
}

